import rclpy                                      # ROS2 Python接口库
from rclpy.node   import Node                     # ROS2 节点类
from rclpy.action import ActionServer             # ROS2 动作服务器类
from move_circle_action.action import MoveCircle    # 自定义接口
from geometry_msgs.msg import Twist
import time
import math

class MoveCircleServer(Node):
    def __init__(self, name):
        super().__init__(name)
        self.server = ActionServer(self, MoveCircle, 'move_circle', self.execute_callback) # 接受客户端请求
        self.publisher = self.create_publisher(Twist, "/turtle1/cmd_vel", 10)        # 控制海龟的话题
        self.info = self.get_logger().info
        self.time_period = 1
        self.radius = 2
        self.step = 20

    def execute_callback(self, aim_action:MoveCircle):
        self.info('Moving circle...')
        feedback_msg = MoveCircle.Feedback()

        for i in range(0, self.step):
            # 控制海龟
            twist = Twist()     
            omiga = 2 * math.pi / self.step         # 角速度 = 2 * pi/ 步数
            twist.linear.x = self.radius * omiga    # 线速度 = 角速度 * 半径
            twist.angular.z = omiga
            self.publisher.publish(twist)
            
            # 响应客户端
            feedback_msg.state = (i + 1) * int(360/self.step)
            self.info('Publishing feedback: %d' % feedback_msg.state)
            aim_action.publish_feedback(feedback_msg)

            time.sleep(self.time_period)
        
        aim_action.succeed()
        result = MoveCircle.Result()
        result.finished = True                            
        return result 

def main(args=None):
    rclpy.init(args=args)
    node = MoveCircleServer("action_move_server")
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()